#include "MPU6050.h" // Device header

// 初始化MPU6050
uint8_t MPU6050_Init(void)
{
    //	HAL_UART_Transmit(&huart1,"here is successful",20,100);
    uint8_t res;
    uint8_t data = 0x00; // 定义数据变量，避免直接传常量地址

    // 唤醒MPU6050（解除睡眠模式）
    HAL_I2C_Mem_Write(&hi2c2, MPU_IIC_ADDR_W, MPU_PWR_MGMT1_REG, 1, &data, 1, 1000);
    HAL_Delay(100);

    // 读取设备ID，确认连接正常
    HAL_I2C_Mem_Read(&hi2c2, MPU_IIC_ADDR_W, MPU_DEVICE_ID_REG, 1, &res, 1, 1000); // 注意从机地址用写地址（MPU6050兼容）

    // 配置陀螺仪量程为±2000°/s（0x18对应量程选择）
    data = 0x18;
    HAL_I2C_Mem_Write(&hi2c2, MPU_IIC_ADDR_W, MPU_GYRO_CFG_REG, 1, &data, 1, 1000);
    HAL_Delay(10);
    // 配置加速度计量程为±16g（0x18对应量程选择）
    data = 0x18;
    HAL_I2C_Mem_Write(&hi2c2, MPU_IIC_ADDR_W, MPU_ACCEL_CFG_REG, 1, &data, 1, 1000);
    HAL_Delay(10);

    // 设置采样率为1kHz（分频系数7，1000/(7+1)=125Hz？根据手册调整）
    data = 0x07;
    HAL_I2C_Mem_Write(&hi2c2, MPU_IIC_ADDR_W, MPU_SAMPLE_RATE_REG, 1, &data, 1, 1000);
    HAL_Delay(10);

    return 0;
}

// 读取MPU6050的加速度数据
void MPU6050_Read_Accel(int16_t *ax, int16_t *ay, int16_t *az)
{

    uint8_t buf[6];
    int16_t ax_raw, ay_raw, az_raw;
    // 对于±16g量程，灵敏度为 32768 LSB/g
    const float sensitivity = 32768.0f;
    const float g = 980.665f; // 重力加速度，单位：m/s²
    // 读取加速度数据寄存器（6个字节）
    HAL_I2C_Mem_Read(&hi2c2, MPU_IIC_ADDR_R, MPU_ACCEL_XOUTH_REG, 1, buf, 6, 1000);

    // 合成16位数据
    ax_raw = (buf[0] << 8) | buf[1];
    ay_raw = (buf[2] << 8) | buf[3];
    az_raw = (buf[4] << 8) | buf[5];

    *ax = (ax_raw / sensitivity) * g;
    *ay = (ay_raw / sensitivity) * g;
    *az = (az_raw / sensitivity) * g;
}

// 读取MPU6050的陀螺仪数据
void MPU6050_Read_Gyro(int16_t *gx, int16_t *gy, int16_t *gz)
{

    uint8_t buf[6];
    int16_t gx_raw, gy_raw, gz_raw;
    const float sensitivity = 16.4f;

    // 读取陀螺仪数据寄存器（6个字节）
    HAL_I2C_Mem_Read(&hi2c2, MPU_IIC_ADDR_R, MPU_GYRO_XOUTH_REG, 1, buf, 6, 1000);

    // 合成16位数据
    gx_raw = (buf[0] << 8) | buf[1];
    gy_raw = (buf[2] << 8) | buf[3];
    gz_raw = (buf[4] << 8) | buf[5];

    *gx = gx_raw / sensitivity;
    *gy = gy_raw / sensitivity;
    *gz = gz_raw / sensitivity;
}

// 读取MPU6050的温度数据
int16_t MPU6050_Read_Temp(void)
{
    uint8_t buf[2];
    int16_t temp;

    // 读取温度数据寄存器（2个字节）
    HAL_I2C_Mem_Read(&hi2c2, MPU_IIC_ADDR_R, MPU_TEMP_OUTH_REG, 1, buf, 2, 1000);

    // 合成16位数据
    temp = (buf[0] << 8) | buf[1];

    // 转换为摄氏度 (计算公式来自MPU6050数据手册)
    return (int16_t)((temp / 340.0) + 36.53);
}

void MPU6050_Calibrate(MPU6050_CalibrationData *cal_data, uint16_t samples)
{
    int32_t ax_sum = 0, ay_sum = 0, az_sum = 0;
    int32_t gx_sum = 0, gy_sum = 0, gz_sum = 0;
    int16_t ax, ay, az, gx, gy, gz;
    uint16_t i;

    for (i = 0; i < samples; i++)
    {
        MPU6050_Read_Accel(&ax, &ay, &az);
        MPU6050_Read_Gyro(&gx, &gy, &gz);

        ax_sum += ax;
        ay_sum += ay;
        az_sum += az; 
        gx_sum += gx;
        gy_sum += gy;
        gz_sum += gz;

        HAL_Delay(10); // 采样间隔
    }

    cal_data->ax_offset = ax_sum / samples;
    cal_data->ay_offset = ay_sum / samples;
    cal_data->az_offset = az_sum / samples;
    cal_data->gx_offset = gx_sum / samples;
    cal_data->gy_offset = gy_sum / samples;
    cal_data->gz_offset = gz_sum / samples;
}
void MPU6050_ApplyCalibration(int16_t *ax, int16_t *ay, int16_t *az,
                              int16_t *gx, int16_t *gy, int16_t *gz,
                              MPU6050_CalibrationData *cal_data)
{
    *ax -= cal_data->ax_offset;
    *ay -= cal_data->ay_offset;
    *az -= cal_data->az_offset;
    *gx -= cal_data->gx_offset;
    *gy -= cal_data->gy_offset;
    *gz -= cal_data->gz_offset;
}
